#include "motor.h"


Motor_t _motor[4];


void motor_init(void)
{   
    // 初始化
    memset(_motor,0,sizeof(_motor));

    _motor[MOTOR_LF].num = MOTOR_LF;
    _motor[MOTOR_LB].num = MOTOR_LB;
    _motor[MOTOR_RF].num = MOTOR_RF;
    _motor[MOTOR_RB].num = MOTOR_RB;

    // 设置pid
		
    set_PID_MaxOutputVal(&_motor[MOTOR_LF].pid, 4000);
    set_PID_P_I_D(&_motor[MOTOR_LF].pid, 2, 5, 0.02);
    set_PID_TargetVal(&_motor[MOTOR_LF].pid, 0);
	
    set_PID_MaxOutputVal(&_motor[MOTOR_LB].pid, 4000);
    set_PID_P_I_D(&_motor[MOTOR_LB].pid, 2, 5, 0.02);
    set_PID_TargetVal(&_motor[MOTOR_LB].pid, 0);
	
    set_PID_MaxOutputVal(&_motor[MOTOR_RF].pid, 4000);
    set_PID_P_I_D(&_motor[MOTOR_RF].pid, 2, 5, 0.02);
    set_PID_TargetVal(&_motor[MOTOR_RF].pid, 0);
		
    set_PID_MaxOutputVal(&_motor[MOTOR_RB].pid, 4000);
    set_PID_P_I_D(&_motor[MOTOR_RB].pid, 2, 5, 0.02);
    set_PID_TargetVal(&_motor[MOTOR_RB].pid, 0);


}

void motor_control(void)
{
    uint8_t i;
    for(i = 0; i < MOTOR_SUM; i++)
    {
        _motor[i].pwm = IncrementPID_Realize(&_motor[i].pid, _motor[i].rpm);
        motor_set_pwm(_motor[i].num, _motor[i].pwm);
    }
}
    

/**
 * @brief 设置电机的PWM
 * 
 * @param motor_num 
 * @param pwm 
 */
void motor_set_pwm(MotorNum_e motor_num, float pwm)
{
    MotorDir_e motor_dir; // 电机方向
    uint16_t motor_pwm; // 电机速度
    if(pwm > 0)
    {
        motor_dir = MOTOR_POSITIVE;
        motor_pwm = pwm;
    }
    else if(pwm < 0)
    {
        motor_dir = MOTOR_REVERSE;
        motor_pwm = -pwm;
    }
    else 
    {
        motor_dir = MOTOR_STOP;
    }

    _motor[motor_num].dir = motor_dir;

    switch (motor_num)
    {
        case MOTOR_LF:
            MotorA_Control(motor_dir,motor_pwm);
            break;
        case MOTOR_LB:
            MotorC_Control(motor_dir,motor_pwm);
            break;
        case MOTOR_RF:
            MotorB_Control(motor_dir,motor_pwm);
            break;
        case MOTOR_RB:
            MotorD_Control(motor_dir,motor_pwm);
            break;
        default:
            break;
    }

}

void motor_set_rpm(MotorNum_e motor_num, float rpm)
{
		_motor[motor_num].rpm = rpm;
}


void motor_set_targetval(MotorNum_e motor_num, float targetval)
{
		_motor[motor_num].pid.TargetVal = targetval;
}


